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ABSTRACT 

This  paper  analyzes  the  problem  of  tracking  targets  in 
noisy  conditions. 

First  a  basic  background  is  provided.  That  includes 
general  concepts  from  estimation  theory  and  a  specific 
description  of  the  Kalman  filter  and  its  use  for  treating 
the  various  aspects  of  the  target  tracking  problem. 

Then  progressively  more  difficult  situations  of  target 
tracking  examples  are  simulated  and  the  results  are 
analyzed  and  compared  with  the  literature. 
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I.  INTRODUCTION 


The  tracking  problem  today  is  of  great  interest  particu¬ 
larly  in  the  military  area.  The  term  "tracking"  is  considered 
to  be  the  processing  of  measurements  obtained  from  a  target 
in  order  to  maintain  an  estimate  of  its  current  state.  The 
latter  usually  contains: 

a.  Kinematic  components  (position,  velocity,  acceleration, 
etc . )  . 

b.  Other  components  (radiated  strength  of  signal,  spec¬ 
tral  characteristics,  etc.). 

c.  Constant  or  slowly-varying  parameters  (propagation 
velocity,  coupling  coefficients,  etc.). 

A  track  is  a  state  trajectory  estimated  from  a  set  of 
measurements  which  are  associated  with  the  same  target. 

Measurements  are  noise-corrupted  observations  related 
to  the  state  of  the  target.  For  example  a  measurement  can 
be  direct  estimate  of  position,  range,  bearing,  time  differ¬ 
ence  of  arrival,  frequency  of  narrow  band  signal,  or  frequency 
difference  of  arrival. 

Many  approaches  have  been  developed  for  the  solution  of 
the  tracking  problem.  Many  of  them  are  very  reliable  and 
work  with  satisfactory  results.  The  problem  however  is 
complicated,  so  there  is  always  need  for  further  improvement 
of  the  tracking  procedures. 


The  complexity  of  the  tracking  problem  is  due  to  the 
following' main  reasons: 

a.  Presence  of  countermeasures  (evasive  maneuvers, 
decoys ,  etc. ) . 

b.  Need  of  advanced  information  processing  techniques 
for  manipulation  of  the  numerous  and  complex  data  to 
be  possible. 

c.  Uncertainty  associated  with  the  measurements  related 
to  the  origin  of  the  measurements,  i.e.,  a  measure¬ 
ment  may  not  have  originated  from  the  target  of  interest. 

d.  Inaccurate  measurements  because  of  random  noise. 

e.  Random  false  alarms  in  the  detection  process. 

f.  Clutter  because  of  reflectors  or  radiators  near  the 
target  of  interest. 

g.  Interfering  targets. 

The  basic  components  of  a  tracking  system  are  shown  in 
Figure  1.1. 

The  term  "filter"  is  often  applied  to  devices  or  systems 
which  process  incoming  signals  or  other  data  in  such  a  way 
as  to  eliminate  noise,  to  smooth  signals,  or  to  predict  the 
input  signal  from  instant  to  instant.  There  is  much  litera¬ 
ture  covering  the  theories  of  estimation,  identification, 
modeling,  prediction,  etc.  The  design  of  such  filters  falls 
in  the  domain  of  optimal  filtering,  which  originated  with 
the  work  of  Wiener  [Ref.  11  and  was  extended  by  the  work  of 
Kalman-Busy  [Ref.  2]  and  others.  It  was  only  around  1970 


that  the  systematic  treatment  of  tracking  multiple  targets 
in  the  presence  of  false  alarms  using  Kalman  filtering 
techniques  has  started  with  the  work  of  [Ref.  3]  and  [Ref.  4]. 

It  has  been  generally  accepted  that  the  Kalman  filtering 
technique  is  one  of  the  most  desirable  of  existing  tracking 
algorithms.  The  reason  is  that  it  is  able  to  accept  many 
inputs  (bearing,  range,  course,  speed,  doppler,  etc.).  The 
inputs  can  be  available  from  a  variety  of  locations  and 
sensors.  The  Kalman  filter  then  weights  them  properly  with 
respect  to  their  errors  and  generates  an  estimate  of  target 
position,  course,  and  speed.  It  contains  also  information 
within  its  structure  sufficient  to  give  the  user  or  decision 
maker  a  useful  indicator  of  the  estimate's  quality. 

The  purpose  of  this  paper  is  to  describe  the  target 
tracking  problem  under  noisy  conditions.  The  Kalman  filter 
is  used  as  the  basic  tool  to  treat  this  problem.  Some  char¬ 
acteristic  problems  are  presented  and  solved. 

In  Chapter  II  some  basic  concepts  and  definitions  are 
provided  from  estimation  theory.  The  contents  of  this  chap¬ 
ter  form  the  theoretical  background  needed  for  the  rest  of 
the  chapters.  It  is  assumed  that  the  reader  is  familiar 
with  basic  probability  theory. 

Chapter  III  contains  a  general  description  of  the  Kalman 
filter  (discrete  and  continuous)  and  the  nonlinear  case 
where  a  linearization  of  target  tracking  problem  takes  place 
for  the  application  of  Kalman  filter  to  be  possible  (Extended 
Kalman  Filter) . 


In  Chapter  IV,  a  brief  analysis  of  the  error  covariance 


matrix  and  the  information  contained  in  it  .has  been  made. 

Also  its  use  in  setting  error  ellipsoids  (confidence  regions) 
about  an  estimated  target  position  has  been  described. 

In  Chapter  V  the  maneuvering  target  problem  is  described 
and  one  of  the  various  existing  approaches  is  analyzed, 
named  "The  White  Noise  Model  with  Adjustable  Level."  The 
same  method  is  used  later  in  Chapter  VI,  in  a  relative  exam- 
!  pie.  Chapter  VI  contains  three  characteristic  examples 

covering  the  application  of  Kalman  filter  in  some  individual 
cases  of  target  tracking  problems.  Specifically  the  presented 
!  examples  are  the  following: 

a.  Linear  target  problem  (measurements  of  target’s  posi¬ 
tion  available) . 

i 

|  b.  Non-linear  target  problem  (measurements  of  target's 

bearing  available) . 

!  c.  Linear  maneuvering  target's  problem  (measurements  of 

j  position  available) .  The  target  maneuvers  two  times 

with  different  acceleration  each  time. 

The  relative  calculations  are  provided  analytically 
together  with  the  corresponding  computer  programs.  The 
resulting  plots  are  also  provided  with  proper  comments. 


II.  SOME  DEFINITIONS  AND  BASIC  CONCEPTS 
FROM  ESTIMATION  THEORY 

The  following  definitions  and  concepts  from  probability 
and  estimation  theory  respectively  are  described  in  detail 
in  Reference  5  and  Reference  6 . 

A.  DEFINITIONS 

1 .  Bayes 1  Rule  for  Random  Variables 

For  random  variables  Bayes'  rule  is  given  by: 


P(x|y) 


P  (y  1  x)  P  (x)  =  P(v|x)P(x) 

P  ( y )  J  P  (y  |  x)  P  (x)  dx 


(2.1) 


The  conditional  probability  of  an  event  is  sometimes  referred 
to  as  "posterior"  while  the  unconditional  one  as  "prior. " 

In  this  case  p(x)  is  called  the  "prior"  probability  density 
function  (p.d.f.)  and  p(x|y)  is  the  posterior  one. 

2 .  Gaussian  Random  Variables 

The  p.d.f.  of  a  Gaussian  random  variable  is 

_  2 

p(x)  =  N(x;x,a2)  =  ~~~  exp{"  ^x-  ~  V  ■}  (2.2) 

/2no  2a 

where  N(  )  represents  the  normal  density  with  argument  x, 
mean  x  and  variance  a. 


An  equivalent  expression  is 


where  x  is  normally  distributed  with  respective  mean  and 
variance. 

3 .  White  Noise 

A  discrete  Gaussian  random  process  v  is  called  "white 
noise"  if  for  any  k  time  points  t^ , t^ , . . . , t^ ,  the  random 
vectors  v(t^)  ,  .  .  .  , v(tjc)  (which  are  Gaussian)  are  independent. 
The  autocorrelation  of  this  random  process  is  zero  for  any 
two  different  times. 

A  discrete  Gaussian  random  process  is  often  used  as 
a  model  for  measurement  noise  and  disturbance  noise. 

4 .  Markov  Processes 

The  common  property  of  Markov  processes  is  the 
following : 

p[x(t)|x(t),  T  < t1J  =  p[x (t)  jx (tx) ]  V  t  >  t1  (2.4) 

i.e.,  the  past  up  to  t,  is  completely  described  by  the  value 
of  the  process  at  t. 

The  state  of  a  dynamic  system  with  input  a  white  noise 

x(t)  =  f [x (t) ,n (t) ]  (2.5) 

is  a  Markov  process. 

5 .  Random. Sequences 


A  random  sequence,  or  a  discrete  time  stochastic 
process,  is  a  time-indexed  sequence  of  random  variables. 


X  =■  (x(j),  j  =  1 , .  .  .  ,  k }  k  =  1,... 


A  random  sequence  is  Markov  if 


p [x (k) I ]  =  p[x(k)  j x ( j ) ]  V  k  >  j 


(2.6) 


(2.7) 


The  sequence  v(j),  j  =  1,...,  is  a  (discrete  time) 


white  noise  if 


E  [ v  (k)  v  ( j  )  ]  =  q5kj 


where  6.  ,  is  the  Kronecker  delta  function: 


(2.8) 


■ 


1  if  k  =  j 


0  if  k  /  j 


(2.9) 


The  state  of  a  dynamic  system  excited  by  white  noise 


x(k+l)  -  f [k , x (k) , v (k) ]  (2.10) 


is  a  discrete-time  Markov  process  or  Markov  sequence. 

A  stochastic  process  x(t),  t  e  I  is  a  Gaussian  white 
(or  independent)  process  if  for  any  m  time  points  t^,...,tm 
in  I  (m  =  integer),  the  m  random  n  vectors  x (t^ ),..., x ( tm) 
are  independent  Gaussian  random  vectors. 

The  state  of  a  linear  dynamic  system  excited  by  white 
Gaussian  noise 


x  (k+1 )  =  F  x  (k)  +  v  (k) 


[2. ii ; 


is  a  Gauss-Markov  process. 


B.  BASIC  CONCEPTS  IN  ESTIMATION 


1.  Estimating  Problem--Random  and  Non-random  Parameters 


The  problem  of  estimating  a  (time-invariant)  parameter 


x  is  : 


Given  the  measurements 


z(j)  =  h [ j ,x ,w( j ) ]  , 


(2.12; 


made  in  the  presence  of  random  noises  w(j),  find  a  function 


x  (k)  =  x  (k ,  z  ) 


(2.13; 


where 


z  =  {z(j),j=l,...,k} 


(2.14) 


that  estimates  the  value  of  x  in  some  sense. 

The  function  (2.13)  is  called  an  estimator  and  its 
value  is  called  the  estimate.  We  can  use  two  models  for  a 
time-invariant  parameter: 

a.  Non-random:  We  have  an  unknown  true  value  Xq .  Here 
it  is  desired  that  the  estimates  converge  in  some  sense  to 
this  true  value  as  k  ®  (non-Bayesian  approach)  . 


b.  Random:  The  parameter  is  a  random  variable  with  a 
prior  p.d.f.  p(x).  In  this  case  a  realization  of  x  accord¬ 
ing  to  p(x)  is  assumed  to  have  taken  place;  It  is  desired 
for  each  measurement  sequence  to  yield  an  estimate  that 
converges  to  the  corresponding  realization  of  x  and  this 
should  hold  for  all  x  (Bayesian  approach) . 

In  the  second  approach  given  the  prior  p.d.f.  of  the 
parameter,  its  posterior  can  be  obtained  using  the  Bay's 


rule . 


In  the  first  case  the  likelihood  function 


Ak(x)  =  p(Z  jx) 


(2.15) 


is  used  as  a  measure  of  how  "likely"  a  parameter  value  is 
for  the  observations  that  are  made. 

A  usual  method  of  estimation  of  non-random  parameters 
is  the  maximum  likelihood  method.  Its  maximum  likelihood 
estimate  (MLE)  is 


x  (k) 


=  arg  max  p(Z  |x) 


2.16) 


The  corresponding  estimate  for  a  random  parameter  is 
the  maximum  a  posteriori  (MAP)  estimate 


MAP  ... 
x  (k) 


arg  max  p (x  j  Z  ) 
x 


arg  max  Ip ( z  j x) p (x) ]  (2.17) 

x 


",  *  .  *  .  *  *  *  ’  •  V.  ’  «  *.*,'• 


.*• .  •  -  v  v  v  .*  -  v  v  •  *  *  -  • .  •  -  •  v*  -  •  •/  •  •  •.  •.*  v  •;  v'  ■  *.  •.  %\ 


23 


The  x  will  coincide  with  x"  for  a  certain  prior 
p.d.f.,  called  "diffuse." 

2 .  Linear  Estimation--Static  Case 

The  minimum  mean  square  error  (m.m.s.e.)  estimate  of  i 
random  variable  x  in  terms  of  another  random  variable  y 
is  the  conditional  mean  E[xjy].  In  practice  the  evaluation 
of  the  conditional  mean  is  complicated  so  usually  the  "linear 
m.m.s.e.  estimation"  method  is  applied.  According  to  this 
method  the  best  estimate  is  such  that  it  is  unbiased  and  the 
estimation  error  is  uncorrelated  from  the  observable (s) , 
i.e.,  they  are  orthogonal. 

The  estimation  of  two  random  vectors  x  and  z  jointly 
normally  (Gaussian)  distributed  is  now  examined: 

Let 


and  covariance  matrix  (assumed  nonsingular) 


where 


Pxx  =  E((X-X)(X-X)'] 


(2.21) 


and 


~X2  =  EHx  -x)  (z  -  z')  ]  (2.22) 

Then  the  m.m.s.e.  estimate  of  x  in  terms  of  z  is 


/V  « 

x  =  .  E  [x  |  z] 


x  + 


( z  -  z) 


(2.23) 


and  the  corresponding  covariance  matrix  is 


*xs 


4  E 


(x  -  x)  (x  -  x)  'I  z  ] 


p  -  p  P-1  p 
—XX  — xz  zz  — zx 


(2.24) 


We  can  see  that  the  optimal  estimate  is  a  linear  function  of 
z  (this  is  because  of  the  Gaussian  assumption) ,  while  the 
covariance  which  measures  the  "quality”  of  the  estimate  is 
independent  of  the  observation  z. 

If  the  random  variables  are  not  Gaussian,  then  the 
"best  linear"  estimate  of  x  in  terms  of  z  (lin.  m.m.s.e.) 


The  m.s.e.  matrix  corresponding  to  Equation  (2.25)  is  given 

by 


E  [  xx '  ] 


P  -  P  P_1  P  =  P 
—xx  — xz  — zz  — ZX  — XX  z 


(2.26! 


From  the  above  results  it  follows  that  the  best  estimator 
(in  the  m. m.s.e.  sense)  for  Gaussian  random  variables  is  the 
same  as  the  best  linear  estimator  for  arbitrarily  distributed 

t 

random  variables  with  the  same  first  and  second  moments. 

3 .  Estimating  with  the  "Least  Squares"  Method 

The  least  squares  method  is  another  common  estimation 
procedure  for  non-random  parameters.  For  measurements 


z(j)  =  h(j,x)+w(j)  (2.27) 

The  least  square  estimate  of  x  is 

k 

xLS(k)  =  arg  min  \  [z(j)  -h(j,x)]2  (2.28) 

x  j  =  l 

Equation  (2.28)  does  not  make  assumptions  about  the  noises 
w(j).  If  these  noises  are  independent  identical  distributed 
(i.i.d.)  zero-mean  Gaussian  random  variables,  then  Equation 
(2.28)  coincides  with  the  MLE  for  these  assumptions. 


The  corresponding  case  for  random  parameters  is  the 
minimum  mean  square  error  (m.m.s.e.)  estimate 


"'MMSE  . ,  ,  _  ,  .  '  .  2  ,  _k, 

a  (k)  =  arg  min  E[(x  -x)  jz  ] 


(2.29) 


The  solution  to  the  above  is 


-MMSE (k)  =  E[x|zk]  =  /  X  p(xiZk)  dx  (2.30: 


where  the  expectation  is  with  respect  to  the  conditional 


p  •  d  •  f  •  • 


P(x| Zk) 


P(Z  lx)P(x) 
P(Zk) 


(2.31) 


4 .  Consistent  Estimators 

For  a  non-random  parameter  case,  the  estimator  is 
called  consistent  if  the  estimate  converges  to  the  true 
value  in  some  stochastic  sense.  Using  the  convergence  in 
mean  square, 


lim  E {  [x  (k) 
k-*-«° 


-  V  > 


(2.32) 


The  expectation  is 


E  [x  ( Z  )]  *  x, 


(2.33) 
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Requirement  for  the  convergence  of  the  estimator 


in  the  random  parameter  case  (m.s.  sense)  is 


1  im  E  {  [  x  ( k )  -  x  ]  2  }  =  0 


(2.34) 


The  expectation  here  is  over  Z  and  x. 


It 


KALMAN  FILTER  AND  APPLICATION  TO  TARGET  TRACKING 


A.  BASICS  ON  KALMAN  FILTER 

Kalman  filtering  technique  is  very  popular  in  target 
tracking  applications.  It  is  basically  a  method  of  solving 
the  problems  of  optimal  filtering  and  prediction,  which 
according  to  Reference  1  is  defined  as: 

a.  Optimal  filtering  is  the  estimation  of  state  vector 
X(t)  from  data  Z_(t)  where  r  <  t. 

b.  Prediction  is  the  estimation  of  a  state  vector 'X(t) 
at  time  t  from  data  Z(t),  where  7  <  t. 

In  target  tracking  both  the  above  problems  are  encoun¬ 
tered.  The  Kalman  filter  is  desirable  because  as  an  estima 
tion  model  it  has  the  following  features: 

a.  At  time  t,  the  filter  generates  an  unbiased  estimate 

A 

X ( t )  of  the  state  vector  X(t) .  That  means  that  the 
expected  value  of  the  estimate  is  the  value  of  the 
state  vector  at  time  t. 

b.  The  estimate  is  a  minimum  variance  estimate  (i.e.,  it 
has  the  property  that  its  error  covariance  is  less 
than  or  equal  to  that  of  any  other  linear  unbiased 
estimate) . 


c.  The  filter  is  recursive  and  it  does  not  store  past 


d.  The  filter  is  linear  which  simplifies  calculations 

and  lends  itself  to  machine  computations.  Gelb  [Ref. 
3]  discusses  the  above  features. 

B.  DISCRETE  KALMAN  FILTER 

Assuming  that  we  are  dealing  with  a  discrete  time  system 
of  the  form: 

X(k+1)  =  £(k+l,k)  X(k)  +  i(k+l,k)  u(k)  +  W(k)  (3.1 

Z (k)  =  C  (k)  X (k)  +  V(k)  (3.2 

the  following  declaration  must  be  done: 

”X(j  |  j ) ”  is  read  as:  "X  hat  of  j  given  j,"  which  means 
that  "The  estimate  of  X  at  time  j  given  measurements  at 
times  up  to  and  including  time  j." 

Then  there  are  eight  main  components  that  make  up  the 
Kalman  filter: 

a.  X(k)  is  the  state  vector  at  time  k,  and  X(k)  is  the 
state  vector  estimate  which  is  an  unbiased  minimum 
variance  estimate  of  the  true  state  vector  X(k)  at 
time  k. 

b.  The  error  covariance  matrix  P(k)  is  a  matrix  repre¬ 
senting  the  covariance  of  the  difference  between  the 
true  state  vector  X(k)  and  the  estimate  X(k) ,  and 


can  be  expressed  as: 


The  transition  matrix  £(k)  is  used  in  the  calculation 
of  the  state  vector  estimate  at  the  present  point,  in 
time  (k+ljk),  from  the  state  vector  estimate  of  a 
past  point  in  time  (k!k).  It  is  also  used  in  the  calcu 
lation  of  the  error  covariance  matrix  at  time  (k  +  1  k: , 
using  the  error  covariance  matrix  at  time  (k  k) . 

The  measurement  vector  Z_(k)  ,  is  the  data  sample  or 
observation  taken  at  time  k  and  its  components  are 
linear  combinations  of  the  components  of  the  state 
vector  X (k)  which  have  been  corrupted  by  uncorrelated 
Gaussian  noise  V(k)  whose  mean  is  zero  and  has  a 
covariance  matrix  R(k) . 

The  covariance  matrix  R(k)  associated  with  the  Gaussian 
noise  that  corrupts  the  observation  or  measurement  at 
time  k,  must  be  provided  by  the  user.  It  is  the  covari 
ance  of  a  sensor  measurement  vector  Z(k) .  For  example 
a  range  sensor  may  have  a  measurement  variance  of  60 
yards.  A  measurement  made  with  that  sensor  would  have 
a  scalar  value  of  60  yards  for  R(k) .  As  will  be  seen 
later,  it  is  important  that  the  value  of  R(k)  be 
accurate  as  it  affects  the  performance  of  the  filter. 
The  conversion  matrix  C(k)  describes  the  linear  com¬ 
binations  of  the  components  of  the  state  vector  X(k) 


which  makes  the  components  of  the  measurement  vector 
Z_(k)  .  The  relationship  between  the  measurement  vector 
Z_(k)  ,  the  conversion  matrix  C(k)  ,  and  the  Gaussian 
noise  V(k)  is: 

Z(k)  =  C (k)  X (K)  +  V(k)  (3.4 

g.  The  Kalman  gain  matrix  G(k)  is  instrumental  in  mini¬ 
mizing  the  difference  between  the  estimate  X(k>  and 
the  state  vector  X(k) .  G(k)  is  chosen  to  minimize 
the  trace  of  the  estimate  error  covariance  matrix 
P(k)  and  is  used  to  revise  the  estimate  X ( k )  of  the 
state  vector  X ( k )  and  the  error  covariance  matrix  P(k) 
after  the  observation  Z(k)  has  been  made. 

h.  The  last  component  to  be  discussed  is  the  system 
dynamics  covariance  matrix  Q(k) .  The  assumption  of 
the  Kalman  model  is  that  the  state  vector  X(t)  exists 
in  a  random  environment  that  is  Gaussian  with  zero 
mean  and  covariance  Q(k) .  The  system  dynamics  covari¬ 
ance  matrix  must  be  input  by  the  user,  and  its 
determination  is  important  as  it  also  affects  the 
performance  of  the  filter. 

The  remainder  of  this  subsection  shows  the  relation¬ 
ships  between  the  algorithm.  Two  important  points  follow: 

a.  The  transition  matrix  b (k) ,  measurement  noise  covari¬ 
ance  matrix  R(k),  systems  dynamics  covariance  matrix 


Q(k)  and  the  conversion  matrix  C(k)  may  be  different 
for  each  point  in  time  k. 

b.  The  filter  must  be  initialized  by  the  user  providing 
the  initial  estimate  X (k)  and  its  associated  estimate 
error  covariance  matrix  P  (k) .  A  poor  initialization 
will  require  more  observations  for  the  algorithm  esti¬ 
mate  to  converge  near  the  value  of  the  state  vector. 

A  Kalman  filter  iteration  can  be  divided  into  phases 
Prediction  and  Filtering. 

During  the  prediction  phase,  the  state  vector  esti¬ 
mate  X(k+ljk)  and  its  error  covariance  matrix  P(k  +  1  k)  are 
updated  from  the  previous  value  at  time  (k  k)  to  the  time 
(k+l|k)  when  the  current  measurement  Z.(k)  is  observed. 

During  the  same  phase  the  system  dynamics  is  introduced  into 
the  algorithm.  The  covariance  matrix  Q(k)  accounts  for  the 
system  dynamics  (environment)  from  time  (k)  to  (k+lk). 

The  update  is  performed  by  multiplying  the  estimate  X(kjk) 
and  the  error  covariance  matrix  P(k|k)  by  the  transition 
matrix  £(k+l,k)  as  follows: 
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X (k+1 | k)  *  £(k+l,k)  X (k | k)  (3.5 

P  (k+1 | k)  =  £(k+l,k)  P(k|k)  i'(k+l,k)  +  Q(k)  (3.6 

If  the  system  has  an  input  u,  the  first  of  the  above 
equations  is  extended  as  follows: 


X  (k+1 1  k)  =  2(;<+1'k)  *(*:*)  +  ^(k+l,k)  u  (k)  ‘  (3.7) 

During  the  filtering  phase,  the  estimate  X(k.k)  of 
the  state  vector  X(k|k)  and  the  error  covariance  matrix 
P(kjk)  are  revised,  based  onthe  latest  measurement  Z_(k) 
observed  at  time  k.  To  do  this,  the  Kalman  gain  matrix 
G(k)  is  first  computed  using  the  error  covariance  matrix 
P(k,k-1)  which  was  updated  in  the  prediction  phase.  The 
covariance  matrix  R(k)  which  is  the  covariance  of  the  Gaussian 
noise  associated  with  the  latest  measurement  Z_(k)  and  the 
conversion  matrix  C(k)  are  also  associated  with  the  latest 
measurement.  The  sequence  of  computations  during  the  filter¬ 
ing  phase  is: 

G (k)  =  P  (k  |  k-1 )  C’ (k)  [C(k)  p (klk-l)C' (k)  +R(k)]_1  (3.8) 

/N  /N  /N 

X (k | k)  =  X (k I k-1)  +  G(k) [Z(k)  -C(k)  X(k|k-1)]  (3.9) 

P (k | k)  =  [I  -G(k)C(k)J  P (k | k-1 )  (3.10) 

The  measurement  prediction  covariance  is 

S (k+1)  =  C(k+l)P(k+l|k)C' (k+1)  +R(k+1)  (3.11) 


The  innovation  or  measurement  residual  is 


v(k+l)  =  z(k+l)  -  z(k+llk) 


(3.12; 


An  important  property  of  the  innovations  is  that 
they  are  an  orthogonal  sequence,  i.e.. 


Eiykv;i  -  skskj 


(3.13) 


where  is  the  Kronecker  delta  function. 

It  must  be  noticed  that,  at  every  stage  k,  the  entire 
past  is  summarized  by  the  "sufficient  statistic  "  x(k  k)  and 
the  associated  covariance.  The  covariance  equations  are 
independent  of  the  measurements.  The  covariance  equations 
can  thus  be  iterated  forward  offline. 

Figure  3.1  shows  a  schematic  of  the  Kalman  filter 
algorithm  and  relationships  among  the  components  described 
above.  Reference  7  provides  a  detailed  discussion  of  Kalman 
filtering  and  optimal  estimation. 


C.  CONTINUOUS  KALMAN  FILTER 

In  order  to  go  from  a  discrete  to  a  continuous  system 
of  the  form: 


X(t)  =  F  ( t )  X  ( t )  +  B  ( t)  W(t) 


(3.14; 


Z(t)  =  C(t)  X(t)  +  V(t) 


(3.15) 
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where  W,V  are  zero  mean,  uncorrelated,  white  noise  processes 
with  covariance  matrices  Q  and  R  respectively,  we  have  to 
consider  the  situation  at  the  limit  as  t(k)  -  t(k-l)  =  it  -  0 . 
At  this  limit  we  have  the  following  equivalences: 

5  (k)  -*■  I  +  F  t  (3.16) 

Q(k)  -  B  Q  B'  it  (3.17) 

R (k)  -  R/At  (3.18) 

R(k)  =  E ( V ( k )  V' (k) )  is  a  covariance  matrix,  while: 

R(t)  =  E  ( V  ( t)  V '  (  t  )  )  =  R(t)c(t-x)  (3.19) 


is  a  spectral  density  matrix.  The  covariance  matrix  R(t):(t  - 
has  infinite  valued  elements.  To  approximate  the  continuous 
white  noise  process  by  the  discrete  white  noise  sequence, 
the  pulse  lengths  (it)  may  be  shrinked  and  their  amplitudes 
may  be  increased,  such  that  R { k )  -*■  R/At.  In  other  words, 
as  At  -*■  0 ,  the  discrete  noise  sequence  tends  to  one  of  an 
infinite  valued  pulse  of  zero  duration,  such  that  the  area 
under  the  "impulse"  autocorrelation  function  is  R(k)At, 
equal  to  the  area  R  under  the  continuous  white  noise  impulse 
autocorrelation  function. 


With  the  above  expressions  in  mind,  the  procedure  is  to 
write  the  proper  difference  equations  and  to  observe  their 


behavior  in  the  limit  as  it 


0.  It  is  assumed  that  R  is 


non-singular,  i.e.,  R  exists. 

Finally  the  continuous  Kalman  filter  equations  are  sum¬ 
marized  as  follows: 

a.  System  Model 

X(t)  =  F  ( t)  X(t)  +  B  ( t )  W(t),  W(t)  -  M(0,Q(t))  (.3.20! 

b.  Measurement  Model 

Z(t)  =  C(t)  X  <  t)  +  V(t)  ,  V(t)  -  N{0,R(t))  (3.21) 

c.  Initial  Conditions 

/\  /V  A 

E[X(0)]  =  Xq,  E[(X(0)  -X0)  (X(0)  -Xq)']  =  Pq  (3.22) 

d.  Assumptions 


R-1 (t) 


exists 


e.  State  Estimate 


X(t)  =  F(t)  X(t)  +  G(t)  [Z(t)  -C(t)X(t)  3  ,  X(0)  =  XQ  (3.23) 


f.  Error  Covariance  Propagation 


P(t) 


F(t)P(t)  +  P(t)F'(t)  +  B  (t)Q(t)  B'  (t)  -  G(t)R(t)G'  (t) 
where  P(0)  =  (3.24) 


Kalman  Gain  Matrix 


g- 


G  ( t )  =  P(t)C'  (t)  R"1  (t)  when  E[W(t)  V'  (  t  )  ]  =  0  (.3.25) 

G ( t )  =  CP ( t) C ’ ( t)  +  B (t) J Ct)  ] R-1 (t)  ,3.26; 

For  more  details,  See  Reference  7,  pp.  119-124. 

D.  EXTENDED  KALMAN  FILTER 

In  target  tracking  it  is  usually  needed  to  estimate  the 
present  target  position,  course  and  speed  and  to  predict 
future  target  position  based  on  the  present  estimate.  The 
Kalman  filter  works  very  well  for  target  tracking  since  the 
algorithm  can  provide  an  unbiased,  minimum  variance  estimate 
of  the  target's  state  based  on  varied  observations  (filter¬ 
ing)  ,  predict  future  position  using  the  prediction  phase  of 
the  filter  and  provide  an  indicator  of  the  estimate  error 
through  the  estimate  error  covariance  matrix. 

Usually  in  practice,  the  state  and/or  measurement  equa¬ 
tions  are  not  linear.  Since  the  Kalman  filter  is  applied  to 
linear  cases,  it  is  necessary  to  find  a  "method"  to  use  it 
in  nonlinear  estimation  problems .  One  approach  is  to  derive 
an  optimal  filter  for  the  nonlinear  problem.  Another  approach 
(more  usual)  is  to  linearize  the  problem  and  apply  Kalman 
filter  theory  to  the  linearized  problem.  The  highlights  of 
the  second  method  are  presented  in  the  following: 


The  system  and  measurement  equations  are  assumed  to  be 


of  the  form: 
Discrete 


x(k+l)  =  a(x(k) ,u(k) ,k)  +  w(k) 

I 

z(k)  =  c (x (k)  )  +  v (k) 


(3.27) 


(3.28) 


Continuous 


x(t)  =  f (x(t) ,u(t) , t)  +  w(t)  (3.29) 

z(t)  =  h (x ( t) )  +  v ( t )  (3.30) 

Here  it  is  assumed  that  we  deal  with  a  discrete  model. 

It  is  assumed  that  we  have  available  a  trajectory 
x(0) (k),  k  =  0,1,2,....  The  vector  function  a (x (k) , u (k) ,k) 
is  expanded  in  a  Taylor  series  about  the  nominal  trajectory 
x(0) (k) .  Then  the  linearized  state  equations  can  be  written 
as : 


x(k+l)  =  A(k)  x(k)  +  a  (x^  ^  (k)  ,u  (k)  ,k)  -  A(k)  x^^  (k)  +  w(k) 

(3.31) 

where  A(k)  is  defined  to  be  the  first  partial  derivatives 
of  the  nonlinear  function  a(x^  (k)  ,u(k)  ,k)  ,  with  respect 


to  the  state  vector  x(k).  i.e., 


3a 


A(k)  =  t— 


9-! (x(0) (k) ,  u  (k) , k) 


(3.32) 


The  accuracy  of  this  approximation  depends  on  the  differ- 


;o) 


ence  between  x  (k)  and  the  actual  trajectory  x(k) .  The 
middle  two  terms  are  treated  as  deterministic  inputs. 

Now  the  measurement  equation  has  been  considered.  We 

have 


2  ( k )  =  c  (x  (k)  )  +  V  (k) 


(3.33) 


The  nonlinear  vector  c  is  expanded  again  about  the  nominal 
(0) 

trajectory  x  (k) .  Then  the  measurement  equation  can  be 
written  as 


(0) 


z (k)  =  H (k) x (k)  +  c(x'  ' (k) )  - 


H(k)x(0)  (k) 


v(k) 


(3.34) 


where  H(k)  is  defined  as  the  first  partial  derivatives  of 
the  measurement  function  c(x^  (k)  )  with  respect  to  the 
state  vector  X(k),  i.e.. 


H  (k) 


3c 


(k) 


(3.35) 


As  in  the  linearized  state  equation,  the  two  middle  terms 
are  known  time-varying  quantities  which  can  be  handled  as  if 


they  were  a  time-varying  measurement  Dias.  For  simplirica- 
tion  it  is  defined 


u*  (k)  =  a(x(0) (k) ,u(k) ,  k)  -A{k)x(0)(k) 


z'(k)  =  z (k)  -c(x(0)(k))  +  H(k)x{0) (k) ) 


(3.36) 


=  z  (k)  -  b  (k) 


(3.37) 


so  that 


x  (k+1)  =  A  (k)  x  (k)  +  u'  (k)  +  w  (k) 


3.38) 


z1  (k)  =  H  (k)  x  (k)  +  v  { k) 


(3.39) 


With  these  linearized  equations,  the  appropriate  Kalman 
filter  equations  are: 
a.  The  gain  equation 


G (k)  =  P(k|k-1)H' (k) [H  (k)  P  (k  j  k-1)  H  ’ (k)  +  R(k) ]  (3.40) 


b.  The  covariance  of  estimation  error  equation 


P (k | k-1)  =  A (k-1 )  P (k-1 | k-1)  A' (k-1)  +  Q(k-l)  (3.41) 


P (k | k)  +  [I-G(k)H(k)J  P (k | k-1) 


(3.42) 


s’*-!-’*!*: -I---!-* 
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c.  Filter  update  equation 


X(kjk)  =  X (k | k-1)  +  G(k)  £z(k)  -c(x(kk-l))]  (3.43) 

d.  Prediction  equation 

x(k+l|k)  =  a  (x  (k  j  k)  ,  u  (k)  ,  k)  (3.44) 

A  block  diagram  of  the  system  and  estimator  is  shown  in 
Figure  3.2. 

The  gains  can  be  pre-computed  and  stored  if  it  is  assumed 
that  the  nominal  trajectory  is  known  in  advance. 

To  answer  the  question,  "where  does  the  nominal  trajectory 
x(k)  come  from?",  two  approaches  are  usually  used. 

In  the  first,  an  approximate  trajectory  is  used.  This 
trajectory  is  known  in  advance  from  known  data  or  may  have 
been  computer  by  solving  the  state  equations 

x(0)  (k+1)  =  a(x^  (k)  ,u(k)  ,k)  (3.45) 

with  the  initial  condition  x^  (0)  *  E[x(0)].  If  the  uncer¬ 
tainty  in  X (0)  is  large,  the  solution  of  the  above  equation 
may  be  "far"  from  x(k)  to  make  the  linearization  sufficiently 
accurate.  If  an  accurate  nominal  trajectory  is  available, 
the  gain  can  be  computed  off-line  and  stored. 

In  the  second  approach ,  the  estimates  produced  by  the 
filter  are  used  as  the  nominal  trajectory  about  which  the 


Nonlinear 
a(x(k) ,u(k) ,k) 


c(x(k) ) 


MODEL 


Figure  3.2  E.K.F.  Block  Diagram  of  the  System  and 
Estimator 


linearization  is  performed.  The  matrices  A (k)  and  H(k)  must 
be  used  to  generate  G(k) .  The  best  trajectory  information 
available  when  H(k)  must  be  evaluated  is  x(k.k-l);  when  A(k) 
is  to  be  evaluated,  x(kjk)  is  available. 

The  filter  that  results  from  using  this  approach  is 
called  an  Extended  Kalman  Filter.  The  gain  and  covariance 
equations  must  be  solved  on-line.  The  procedure  of  the 
filter's  calculations  is  given  below: 

A 

1.  Start  with  x(Oj-l)  and  evaluate  H ( 0 )  using: 

3c 

H(k)  =  ttt  (3.46) 

-  (x(k  k-1)  ) 

2. 

P(0|-1)  =  M  =  E{(x(0|-1)  -  x(0)>  (x(Oj-l)  -x(0))  '  }  (3.47) 

Use  this  matrix  to  compute  G(0)  given  by 
G(k)  =  P(k[k-1)H'  (k)  (H  (k)  P  (k  [  k-1)  H  1  (k)  +R(kn~1  (3.48) 

A 

3.  Compute  x(Q|0)  from: 


x ( o ! o) 


(3.50) 


=  x  ( 0  i  -1 )  +  G(0)  [2(0)  -  c  (x(0  -1)  ) 

and  x ( 1 ] 0)  from: 

A  A, 

X  (k+1 j  k)  =  a (x(k j k) ,u(k) ,k)  (3.51) 

4.  Compute  P(0|0)  from: 

P(kjk)  +  [I-G(k)H(k)]P(kk-l)  ,3.52; 

5.  Since  x(0'0)  is  available  before  A(0)  is  calculated, 

A 

the  value  of  x(0j0)  is  used,  hence: 

3a 

A(0)  -  (3.53) 

-  (x ( 0 j 0) ,u(0) ,0) 

and  then  P(1]0)  is  computed  from: 

P (k | k-1)  =  A(k-l) P(k-1 |k-l) A' (k-1)  +  Q(k-l)  (3.54) 

The  process  is  repeated  by  recycling  through  steps  1  to  5 . 
The  Extended  Kalman  Filter  presented  was  obtained  by  a 
Taylor  series  expansion  up  to  the  first  order  terms.  This 
filter  obviously  introduces  errors  in  the  equations  where  the 
higher  order  terms  are  neglected. 

There  are  several  ways  of  compensating  for  these  errors. 


Very  roughly  ,  an  addition  of  "artificial  process 
noise”  covariance  can  be  made,  for  compensation  of  the  errors 
is  the  state  prediction.  Also  a  multiplication  of  the  state 
covariance  by  a  scalar  1  >  1  at  every  sampling  time  can  be 
carried  out.  This  multiplication  is  equivalent  to  the  filter 
having  a  "fading  memory,"  i.e.,  at  every  sampling  time  the 
past  data  is  "discounted"  by  attaching  to  it  a  higher  covari¬ 
ance  (lower  accuracy). 

The  above  subject  is  covered  with  more  details  in  Reference 
8,  pp.  4-52  —  4.58  and  in  Reference  5,  pp.  3 . 3-1--3 . 4-1 . 


IV.  ERROR  COVARIANCE  MATRIX  AND  TARGET 
TRACKING  QUALITY 

A.  DEFINITION  OF  ERROR  COVARIANCE  MATRIX 

The  error  x  in  the  estimate  of  a  state  vector  is  defined 
to  be  the  difference  between  the  estimated  (x)  and  the 
actual  (x)  values: 


x  =  x  -  x  (4.1) 

t 

The  above  difference  is  known  as  the  error  vector  or 
estimate  error.  The  covariance  of  the  estimate  error  is 

/■v  /V 

P  =  E[  (X(t)  -  X  ( t)  (X(t)  -X(t)  )  1  ]  (4.2) 

It  provides  a  statistical  measure  of  the  uncertainty  in 
x.  It  is  possible  to  discuss  the  properties  of  the  covari¬ 
ance  matrix  independently  of  the  mean  value  of  the  state. 

The  information  contained  is  sufficient  to  generate  indicators 
of  the  estimate  quality. 

B.  INFORMATION  CONTAINED  IN  THE  ERROR  COVARIANCE  MATRIX 
There  are  five  important  characteristics  of  the  error 
covariance  matrix  which  relate  the  matrix  to  the  state 
vector  and  its  estimate. 


(1)  The  error  covariance  matrix  of  an  n-component  state 
vector  is  an  n  by  n  symmetric  matrix. 


(2)  The  diagonal  elements  of  the  error  covariance  matrix 
are  the  mean  square  errors  of  the  -  error  vector 
components . 

(3)  The  trace  of  the  error  covariance  matrix  is  the 
mean  square  length  of  the  error  vector. 

(4)  The  off-diagonal  terms  of  the  matrix  are  correlations 
between  the  elements  of  the  error  vector  X(t)  -  X ( t ) . 

(5)  The  error  covariance  matrix  P(k)  tends  to  the  system 
dynamics  covariance  matrix  QUO,  as  k  goes  to 
infinity.  This  means  that  as  more  information  is 

'available  about  the  state  vector  (observations)  the 
estimate  uncertainty  approaches  the  uncertainty  of  the 
environment  in  which  the  state  vector  exists. 

C.  ERROR  ELLIPSOIDS  ABOUT  A  TARGET  POSITION 

Frequently  it  is  significant  to  have  available  an  indica¬ 
tion  of  the  quality  of  the  estimates.  One  approach  to 
achieve  this  is  the  proper  use  of  the  error  covariance 
matrix  P(k|k).  The  outline  of  this  approach  is  described 
below. 

It  is  assumed  that  the  initial  state  of  the  model  x(0) 
and  the  random  processes  v(k) ,  w(k)  are  Gaussian.  If  this 
assumption  is  satisfied  then  it  can  be  shown  that: 

A  .  /S 

x(k),x(k]k)  and  e(k|k)  =  x(k|k)  -  x(k) 

are  Gaussian.  The  results  obtained  apply  only  to  this  case. 


The  probability  density  function  for  e(k  k)  can  be 


written  as 


f  E  ( e  ( k  ;  k )  1 


(2^) n/2 , P (k  k) 


~Yj2  exp(-ie'(k  k)P  *(k  k)e(k  ki 


-1 


i4.  3) 


For  a  fixed  time  k  this  expression  can  be  written  as 


f  E  ( ^ 


(2;t) 


n/2  1/2 


exp-.-  -e'  we: 


i  -1 

where  w  =  £  (kjk).  In  order  to  determine  the  locus  of 
points  where  the  density  function  fE(e),  has  a  constant 
value  the  above  equation  has  to  be  examined.  It  is  seen 
that  fg(e)  has  a  constant  value  whenever 


constant 


A 


2 


1 


(4.5) 


It  can  be  shown  that  the  points  e  which  satisfy  Equation 
(4.5)  are  hyperellipsoids  (in  two  dimensions,  ellipses). 

If  the  left  side  of  (4.5)  is  expanded  for  the  two-dimensional 
case  (the  same  approach  can  be  extended  to  n  dimensions), 
we  have : 


where  the  symmetry  of  w  has  been  used  (w^2  =  w21^  *  Equation 

2 

(4.6)  is  an  ellipse  (w^  >  0,  w^2  >  0  and  '**22  >  wi2^‘ 

It  is  not  easy  to  be  recognized  as  such  because  its  principa 
axes  do  not  coincide  with  the  coordinate  axes  as  shown  m 
Figure  4.1. 

First  the  principal  axes  must  be  determined  and  then  the 
ellipse  can  be  rewritten  in  terms  of  and  as 

coordinate  axes. 


Figure  4.1  Error  Ellipse 


The  ellipse  in  the  new  coordinate  system  is  described  b 


Xlyl  + 


2 

2^2 


(4. 


where  y^  and  y  ^  are  eigenvectors  of  w  and  ^  and  ^  are 
the  corresponding  eigenvalues. 

As  it  has  already  been  mentioned,  W  =  P  ^ .  Equation  ,4 
is  an  ellipse  in  terms  of  the  eigenvalues  ^  and  ^  and  the 
eigenvectors  v^^  and  y^^  of  W.  The  expression  which  give 
the  ellipse  in  terms  of  the  eigenvalues  and  eigenvectors 
of  P  is 


where  y^  is  an  eigenvector  for  P  and  a^  =  l/.\.  is  the 
corresponding  eigenvalue. 

This  result  can  be  generalized  to  n  dimensions  as  well. 
Given  the  quadratic  form 


(4. 


the  eigenvalues  of  P  are  a^a^.-M^H  and  the  corresponding 
eigenvectors  are  y^  ,y  ^  ^  ,  .  . .  ,y  ^  .  The  quadratic  form 
(Equation  (4.9))  describes  a  hyperellipsoid  which  can  be 


written  as 


+  ...  +  -ii  =  1"  (4.10) 

al  a2  an 

All  vectors  e  in  the  n-dimensional  space  can  be  written  as  a 
linear  combination  of  the  eigenvectors  y  ^  ^  , y ^  >  ,  .  .  .  ,  v  1  n '  . 

The  coefficient  of  y(1)  in  the  linear  combination  is  y  ^ , 

( 2 ) 

the  coefficient  of  y  is  y 2  >  etc. 

So  the  surfaces  of  equal  probability  density  are 
ellipses  (or  hyperellipsoids). 

The  problem  can  now  be  stated  as  follows: 

For  a  specified  value  of  A,  what  is  the  probability  that 
e  lies  within  or  on  the  ellipsoid? 

These  probabilities  have  been  tabulated  below  for  a 
few  values  of  n  and  A. 

TABLE  I 

Probabilities  for  Error  Ellipsoids 

A 


n 

1 

2 

3 

1 

.683 

.  955 

.  997 

2 

.  394 

.  865 

.  989 

3 

.200 

.739 

.  977 

For  example,  for  a  system  having  n  =  2: 

Probability  error  inside  A  =  1  ellipse  =  0.394 
Probability  error  inside  A  =  2  ellipse  =  0.865 


Probability  error  inside  X  =  3  ellipse  =  0.989 

Therefore,  if  the  error  covariance  matrix  P  is  given,  the 
error  ellipsoids  can  be  determined  by  finding  the  eigenvalues 
and  eigenvectors. 

The  error  ellipses  are  useful  in  visualizing  the  estima¬ 
tion  error.  By  using  them  we  can  consider  the  true  state 
value  to  lie  within  a  certain  region  surrounding  the  estimate 
For  details  and  derivations,  see  Reference  8,  pp.  4.44-- 


V.  MANEUVERING  TARGETS 


A.  GENERAL  DESCRIPTION  OF  THE  PROBLEM 

The  maneuvering  target  is  generally  described  by  the 
equation : 


x (k+1)  =  F  (k)  x (k)  +  G (k) u (k)  +  v(k)  (5.1) 

where  the  matrices  F(k) ,  G(k)  are  assumed  known  and  the 
process  noise  v(k)  is  zero  mean,  white  random  sequence  with 
covariance  matrix  Q(k) .  The  main  characteristic  of  the 
maneuvering  target  equation  is  that  the  inputs  u(k)  are 
unknown . 

In  the  following,  linear  models  are  examined  for  sim¬ 
plicity  but-  the  same  techniques  can  be  applied  to  nonlinear 
cases . 

A  number  of  different  approaches  to  the  maneuvering  target 
problem  have  appeared  in  the  literature.  The  most  commonly 
used  model  is,  due  to  simplicity  requirements,  a  kinematic 
model  with  states  consisting  of  position,  velocity,  and  in 
most  cases  also  acceleration,  driven  by  white  noise. 

It  is  possible  to  divide  the  different  approaches  into 
two  broad  categories: 

A.  The  unknown  input  (maneuver  command)  is  modeled  as  a 
random  process. 

B.  The  unknown  input  is  estimated  in  real  time. 
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The  random  process  type  models  can  be  further  classified 
into  two  categories,  depending  on  their  statistical  properties 

Al .  White  noise 

A2 .  Autocorrelated  (Markov)  noise 

All  these  methods  are  approximations  because  in  general, 
the  maneuvers  are  not  stochastic  processes. 

In  the  input  estimation  case  the  assumption  of  having  a 
constant  input  over  a  certain  period  of  time  is  usually  made. 
The  estimation  can  be  based  on  the  least  squares  criterion 
and  can  be  used  in  the  following  ways: 

Bl.  The  estimated  input  corrects  the  state  estimate. 

B2 .  The  input  becomes  an  extra  state  component  (state 

is  augmented)  that  is  reestimated  within  the  state. 

In  the  following  section  only  one  method  is  described. 

This  method  is  illustrated  by  an  example  in  Chapter  VI.  It 
was  selected  from  the  others,  due  to  its  simplicity. 

For  detailed  descriptions  of  different  approaches  to  the 
problem,  see  Reference  5. 

B.  THE  WHITE  NOISE  MODEL  WITH  ADJUSTABLE  LEVEL 

In  this  method  a  certain  low  level  of  process  noise  is 
assumed  in  the  filter. 

A  maneuver  is  considered  as  a  large  innovation.  The 
detection  procedure  is  based  on  the  square  norm  of  the 
innovations 


e„(k) 


V’ (k)S_1 (k)V(k) 


(5.2) 


where 


V (k)  =  z(k)  -  z (k | k-1)  (5.3) 

Based  on  the  target  model  (for  a  non-maneuvering  target) 
a  threshold  is  set  up 

Pts„(k)  <  e  }  =  1  -  a  (5.4) 

V  max 

where  a  is  arbitrary.  For  example,  a  =  0.05. 

If  the  threshold  is  exceeded,  Q(k-l)  is  multiplied  by 
a  scaling  factor  $  until  is  reduced  to  the  threshold 

emax  * 

When  using  the  factor  $  the  covariance  of  the  innovations 
takes  the  form: 

S  (k)  =  C(k)  [F (k-1 ) P (k-1 | k-1) F 1  (k-1)  +  <|>Q(k-l)  ]  C'  (k)  +R(k) 

(5.5) 

This  obviously  reduces  the  value  of  ev(k) . 

An  analogous  technique  is  possible  to  be  used  to  lower 
the  process  noise  level  after  the  maneuver. 


I.  CHARACTERISTIC  COMPUTER  EXAMPLES 


A.  A  LINEAR  KALMAN  FILTER  EXAMPLE 


1 .  Problem  Description 


The  target  is  assumed  to  be  described  by  the  system: 


x  (k+1)  = 


IT  IT 

x  ( k )  +  1  w(k)  k  =  0 , 1 , 

o  ij  !ij- 


(6.1) 


The  available  measurements  are  of  the  type: 


!1  0]x(k)  +  v(k)  k  =  1,2,... 


(6.2) 


where  w(k)  ~  N(0,Q),  v(k)  ~  N(0,R)  are  mutually  independent, 
zero  mean,  white  random  sequences. 

The  initial  state  is 


(6.3) 


Two  measurements  z ( 0 )  and  z(l)  are  made  to  initialize 


a  Kalman  filter. 


The  sampling  time  is  given  as  T  =  0.1. 

The  process  and  measurement  noise  are  to  be  yielded 
by  a  Gaussian  random  number  generator,  given  that  Q  =  R  =  0.0 


Case  1 


After  the  run  of  the  Kalman  filter  for  k  =  2,.. 
the  following  expressions  are  useful  to  be  plotted: 

a.  True  trajectory  vs.  estimated  trajectory  in  the  x 
plane . 

b.  Position  error  variance 

p11(l!l),  p1  ^  ( 2  i  1 )  ,  P]_i  ( 2  ;  2 )  ,  .  .  . 

c.  Normalized  position  error 

* 

[x1(k)  -x1(k|k)  ]/[p  (k|k)  ]1/2  ,  k  =  2,.  ..,100 

d.  Velocity  error  variance 

P22(l|l)/  P22^2|l)»  P22^|2)'**# 

e.  Normalized  velocity  error 

[x2  (k)  -x2(kjk)  ]  /  [p22  (k  |  k)  ]1/2 

f.  Normalized  state  error  squared 
(x(k)  -x(k|k>  ]  ’p'1  (k)  tx(k)  -x(k|k)] 

g.  Normalized  innovation  error 

[z (k)  -z(k|k-l)]/[Pll(k|k-l)  +  R] 1/2 


Case  2 

In  order  to  see  the  effect  of  running  the  Kalman 
filter  with  a  different  Q  than  that  of  the  model,  it  is  helpful 

to  plot  the  same  expressions  as  in  Case  1,  using  a  different 

-2 

Q  for  the  target's  model,  say  Q  =  9  x io 
Case  3 

Finally,  it  is  also  interesting  to  see  the  change 
on  the  same  expressions  as  the  Q  of  both  the  model  and  the 

filter  increases.  For  this  reason  new  plots  of  these  expres- 

-2 

sions  are  to  be  obtained,  using  Q  =  9  < 10  for  both  the  moael 
and  the  filter. 

2 .  Analysis 

a.  True  Trajectory 

The  matrix  form  of  the  given  model  is  converted 
in  the  following  equations: 

x^(k+l)  =  x-^(k)  +  Tx2 (k)  +  Tw(k)  (6.4) 

x2(k+l)  =  x^ (k)  +  w(k)  (6.5) 

where  T  =  0.1,  w(k)  is  generated  by  a  random  number  generator 


function  and 


b.  Measurements 


From  the  given  measurement  equation  it  is 

obvious  that 


z  (k)  =  x^k)  +  v  (k)  (6.6) 

where  x^(0)  =  5  and  v(k)  is  generated  by  a  random  number 
generator  function. 

c.  Derivation  of  x(ll),  p(ll) 

It  is  reasonable  to  start  with  the  following 
estimations  for  the  position  and  velocity  vectors: 


x(l|l)  = 


xx (1|1) 
x2(l|l) 


2(1) 

z (i)  - z (o: 


(6.7) 


then 


Xx  (1 1 1)  =  Xx(l  )  +  v(l) 


(6.8) 


x2(l|l)  = 


Xl(1)  ’xl(0)  .  v(l)  -  v  (0) 

T  T 


(6.9) 


matrix 


Using  these  values  in  the  initial  error--  covariance 


P  <  1 1 1)  =  E 


x‘ (1 | 1) 

x1(l|l)x2 (1 1 1 ) 


x1(l|l)x2(l|l) 
x2  (1 1 1) 


(6.10) 
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where 


x1 ( 1 i 1 )  *  x1(l|l)  -  x1(l;l) 

x2  (1 1 1)  *  x2(ljl)  -  x2(l;l) 


It  is  obtained  that 


P  ( 1  1) 


(6.11) 


(6.12) 


(6.13) 


d.  Run  of  the  Kalman  Filter 

(1)  Calculation  of  Gains.  The  known  equation 
of  the  Kalman  Filter  is: 


P (k  j  k-1)  =  ^P  (k-1 1  k-l)_? '  +  Q'  ( k— 1 )  (6.14) 

It  has  been  converted  in  matrix  form  and  after  some  manipula¬ 
tion,  the  following  equations  are  obtained: 

Pu(k|k-l)  =  PX1  (k-1 1  k-l)  +  TP21(k-l|k-l) 

+  TP12 (k-1 J  k-1)  +  T2p22 (k-1 |k-l)  +  T2g(k))  (6.15) 

P12 (k | k-1)  =  E12 (k-1 | k-1 )  +  TP22  (k-1 | k-1 )  +  TQ ( k )  (6.16) 

P21(k|k-1)  =  P21(k-l|k-l)  +  TP22 (k-1 1  k-1 )  +  TQ(k)  (6.17) 


P22(k|k-1)  =  P22(k-i:k-l)  +  Q (k)  16.18) 

Keeping  the  same  procedure,  the  Kalman  Filter 

equation 

G  (k)  =  P  (k'ik-l)C'  [C  P  (kjk-l)C'  +R(k)]~1  (6.19) 

takes  the  following  form 

Gl(k)  =  P11(k|k-1)  +  R (k)  (6*20) 

P51 (k | k-1 ) 

G2(k)  =  P11(k|k-1)  +  R  (k)  (6-21) 

Finally  the  equation 

P (k | k)  =  [I  -  G(k)C]P(k j k-1)  (6.22) 

is  analyzed  in  the  following: 

P11(k|k)  =  P11(k|k-1)  -G1(k)P11(k|k-l)  (6.23) 

P12(k|k>  *  P12 (k  k-1)  -  G: (k)P12 (k |k-l)  (6.24) 

P 2 1  ( k  ’  k )  =  P2l(k  *-L)  "  52 ik) -11 (k ' k_1)  (6.25) 


P^o (k I k) 


P._ (k | k-1)  -  G-,  (k)  P.  _ (k | k-1) 


(6.26) 


JJ  J.  J.'  II  ■■  ■■.  ■„■. ....  1,1 !  I..  ■■ 

2 .  Estimated  Trajectories 

Following  the  procedure  described  above,  the  estimated 
trajectories  given  by  the  equations: 

A  A 

x(kjk-l)  =  _£  x (k-1  ;  k-1 )  +lu(k)  (6.27) 


x  ( k  j  k)  = 

x(kjk-l)  +  G(k) [z(k)  -  C(k) x(k , k-1) ] 

CX) 

take  the 

form: 

A 

~  /v 

xx  (k  j  k-1) 

=  x,(k-ljk-l)  +  Tx2  (k-1 ;  k-1) 

i6.29) 

x2 (k  j k-1) 

=  x  (k-1 j k-1) 

(6.30) 

A 

x1 ( k  j  k ) 

=  G1(k)z(k)  +  (l-G(k) )x1(kjk-l) 

(6.31) 

x2  (k  |  k) 

A  A 

=  X2(kik-1)  +G2(k)z(k)  -  G2(k)x1(k|k-1) 

(6.32) 

e.  Normalized  State  Error  Squared 

The  normalized  state  error  squared  is  given  by 
the  expression: 

[x(k)  -x(k|k)]'P  1(k)[x(k)  -x(k|k)] 

By  manipulating  this  expression  in  its  matrix 
form  it  is  obtained  that  it  is  equal  to  the  following: 


P11P22  “  P21P12  ^Xi-(k) -Xi  (k  ,  k)  J  P22  +  [x2  (k)  -x2  (k  :  k)  ] 


-  2P21  [x2  (k)  x2  (k)  -  x2  (k)  (k  1  k)  -  x2  (k  ,  k)  x1  (k) 


+  x~ (k  j  k)  x,  (k  j  k) ] ] 


f.  Normalized  Innovation  Error 


By  making  use  of  the  equation 


z  (k  i  k-i; 


c  x (k | k-i: 


(6.33) 


the  given  expression  for  the  innovation  error  becomes 


[2 (k)  -  z(klk-l) ] 

rn  ^  llf-1  1  d  1  1/2 


[  2  (k)  -X1(kjk-1)] 


[Pil(k|k-1)  +R]x/i  [P11(k|k-1)  +R1 


6. 34) 


All  the  above  have  been  properly  set  in  a  FORTRAN 
program  (pp.  178~180)*  The  results  corresponding  to  cases 
1,2,  and  3  are  indicated  on  pages  66-89,  respectively. 

Some  comments  have  been  made  on  the  computer  program  outputs 
on  pages  65  and  90-94. 

3 .  Comments  on  the  Graphs 

a.  Normalized  Expressions  c,  e,  g  of  Cases  1,  2,  and  3 
The  normal  distribution  has  p.d.f.: 


f  (x)  = 


1 ,  —.2  2 

1  ~2 ( x  — x)  /a 


(6.35) 
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Figure  6.12  Normalized  Position  lirror.  Case  2(c) 


Figure  6.15  Normalized  State  Krror  Squared. 
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Figure  6.21  Velocity  Error  Variance.  Case 


Figur 


Figure  6.23  Normalized  State  Krror  Squared. 


Figure  6.24  Normalized  Innovation  Frror .  Casi 


By  standardize  the  normal  distribution  curve  we 
can  have  a  single  curve  that  may  be  adapted  to  all  values  of 
the  mean  as  well  as  differing  values  of  the  standard  deviation. 
This  standardization  may  be  accomplished  by  substituting 
z  =  (x-x)/o  in  Equation  (6.35). 

The  above  concept  is  the  case  for  the  subpara¬ 
graphs  c,  e,  g.  In  Case  c,  for  example, 

z  =  [x1(k)  -  (k  ,  k)  ]  /  t  P  ^  ( k  k)]1//2  . 

The  standard  normal  distribution  curve  has  a 
mean  of  zero  and  variance  and  standard  deviation  equal  to 
one . 

By  integrating  Equation  (6.35)  (after  the  substi¬ 
tution  of  z) ,  it  is  found  that: 

Between  -la  <  z  <  +lo,  68.2%  of  the  area  under  the  curve 
is  included. 

Between  -2a  <  z  <  +2 a,  94.5%  of  the  area  under  the  curve 
is  included. 

Between  -3a  <  z  <  +  3a,  99.74%  of  the  area  under  the  curve 
is  included. 

With  the  above  in  mind,  the  probability  that  the 
normalized  position,  velocity  and  innovation  errors  lie  between 
±2a  is  94.5%  (or  between  ±3o  is  99.74%). 

From  the  graphs  it  is  seen  that  in  the  two 
cases  where  the  value  of  Q  is  common  for  the  model  and  the 


filter,  the  three  normalized  errors  lie  100%  between 
±3a . 

In  the  case  where  Q  is  different  between  the  model 
and  the  filter  it  is  seen  that  these  errors  exceed  at  some 
points  :3a.  But  generally  it  seems  that  the  law  of  99.74% 
between  :3a  is  applied  satisfactory. 

b.  True  Trajectory  vs.  Estimated  Trajectory 

(expr.  a) 

(1)  Q  =  0.02  for  Model  and  Filter  (pp.  66-67} . 
There  is  considerable  difference  between  true  and  estimated 
curve  at  the  beginning  but  as  time  progresses,  there  is  a 
continuous  improvement.  Near  the  end,  the  two  curves  nearly 
coincide . 

( 2 )  Q  =  0.02  for  the  Filter  and  Q  =  0.09  for 
the  Model  (pp.  74-75) .  There  is  a  significant  difference 
between  true  and  estimated  curve  at  the  beginning  which  is 
slightly  improved  at  the  end  of  the  curves. 

( 3 )  Q  =  0.09  for  the  Model  and  Filter  (pp.  82-83) 
Same  as  in  (1) ,  with  the  only  difference  that  at  the  end  of 
the  curves,  the  improvement  is  not  nearly  a  coincidence  but 

it  is  better  than  that  of  Case  (2). 

Generally  the  rate  of  the  improvement  is 
greater  at  small  values  of  time  because  the  gain  is  inversely 
proportional  to  time  (G(k)  =  1/k+l)  and  it  weights  the  correc¬ 
tion  term  [^(k)  -  C(k)x(k|k-1)  less  heavily  as  time  progresses. 

The  fact  that  the  improvement  between  true 


and  estimated  curves  is  less  in  Case  (3)  than  that  of  Case  (1) , 


was  expected.  The  reason  is  that  Q  in  Case  (3)  has  a  greater 
value  and  as  it  is  known,  Q  increases  the  uncertainty  in  the 
one  step  prediction: 


P(kjk-l)  =  i  P (k-1 ; k-1) ; '  +  Q 


16.36) 


This  affects  also  the  P(k|k): 

P  (k  ‘  k)  =  [I  -G(k)c(k)  ]P(kk-l)  ( 6  .  37. 

The  fact  that  the  estimated  curve  in  Case 

(2)  differs  from  the  true  curve  more  than  that  of  Cases  (1) 

and  (3)  and  also  that  the  improvement  is  not  so  significant 

as  in  the  other  cases  was  also  expected.  The  reason  is  that 

the  Q's  of  the  mode-and  the  filter  are  different,  so  this 

difference  affects  the  P  and  G  of  the  filter  negatively, 

resulting  in  a  difficulty  in  approaching  the  true  trajectories 

c.  Normalized  State  Error  Squared  Error  (expr.  f) 

It  is  known  from  the  theory  that  (x  -  x)  'P  (x  - 

is  the  sum  of  the  squares  of  n  independent  zero-mean,  unity 

variance  Gaussian  random  variable,  i.e.,  N(0,1).  This  means 

that  (x-x)'P  1(x  -x)  has  a  chi-square  distribution  with  n 

degrees  of  freedom  (n  is  the  dimension  of  vector  x) . 

2 

If  the  chi-square  distribution  (x  )  equals  zero, 

the  true  and  estimated  state  vectors  agree  exactly.  The 

2 

larger  the  value  of  x  the  greater  the  discrepancy  between 
the  true  and  estimated  state  vectors. 


|X| 


What  is  expected  to  be  seen  in  this  problem  is 
a  greater  discrepancy  in  the  case  where  the  Q's  of  the  model 
and  the  filter  are  different,  due  to  the  poor  estimations  of 
the  state  vectors. 

The  above  expectation  seems  to  be  the  case. 

Specifically  in  the  cases  where  the  same  Q  for  the  filter 

and  the  model  is  used,  it  can  be  said  roughly  that  the 
2  2 

X  Q25  and  X  975  are  approximately  0.06  and  7.5  as  they  should 
be  for  two  degrees  of  freedom  (see  proper  tables) . 

d.  Position  and  Velocity  Error  Variances  (expr.  b,d) 

In  Cases  1  and  2  (filter  has  Q  =  0.02) ,  we  have 
identical  position  error  variances  and  identical  velocity 
error  variances.  After  some  initial  variations  the  position 
error  variance  takes  its  steady  state  which  is  almost  zero 
(expected) ,  while  the  velocity  error  variance  takes  also  its 
steady  state  which  is  approximately  0.04. 

In  Case  3,  the  increase  in  Q,  increases  P^^(klk-1) 
and  reduces  P^(k|k)  (see  proper  equations).  Then  the  steady 
state  values  of  P^(k[k-1)  and  P^^(kjk)  are  different  and 
the  result  is  that  the  position  error  variance  oscillates 
between  the  values  0.0  and  0.01.  For  the  same  reason  the 
velocity  error  oscillates  between  the  values  0.15  and  0.24. 

The  mean  values  of  both  then  are  greater  than  the  corresponding 
values  of  Cases  1,  2.  This  is  logical  since  now  the  Q  is 
greater  as  it  was  derived) 


Position  Error  Variance: 


PX1  (k  j  k)  =  P11(kjk-1)  -  G-j^  (k)  Pi;L  (k ;  k-1)  (6.38) 

P11(kjk-1)  -  — ii (k“l  k~l )  +  TP21 (k-1 ’ k- 1 ) 

+  TP12 (k-1 , k-1)  +  T2P22 (k-1 ! k-1 )  +  T2Q  (6.39) 

Velocity  Error  Variance: 

—2  2 ( k : k )  =  -G2(k)P12(k|k-l)  +  P22 (k i k-1 )  (6.40) 

P22lk|k-1)  =  P22 (k-1 : k-1)  ♦  g  (6.41! 

So  Q  is  proportional  to  P.^  and  P22“ 

B.  AN  EXAMPLE  USING  E.K.F.  (NON-LINEAR  CASE) 

1 .  Problem  Description 

A  stationary  target  is  located  at  =  x2  =  100. 
Bearing  measurements  from  a  moving  ship  are  taken  at 
k  =  1 , . . . , N  from  locations  as  indicated. 

The  following  are  given: 

z (k)  =  9(k)  +  v (k)  (6.42) 

-1  X2 

6(k)  =  tan  — — — -  ^  ^  k  =  1,...,N  (6.43) 

E [ v (k) ]  =  0  (6.44) 


94 


4 fits 

/:Wm\ 

/>//  //'  i 


Target 


Bearings 


'  /. 


Movinc  Shi 


Figure  6.25  Descriptive  Diagram  for  the  Non-linear  Problem 


E  [  v  (k)  v  ( j  )  ]  =  a  5 


(6.45) 


a (k)  =  10k  a2  =  ( 2 0 ) 2  N  =  12 


6.46) 


The  initial  estimate  is: 


x ( o  to: 


xL(o  |  o: 


x2  (0(0) 


(6.47) 


P  (0  I  0)  = 


100  0 


0  100 


(6.48) 


Based  on  the  bearings  of  the  moving  ship,  an  Extended 
Kalman  Filter  can  be  used  to  improve  the  initial  estimate 
of  the  target's  position. 
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UNCLASSIFIED 


F/G  9/2 


NL 


c  (x  (k)  )  =  9  =  tan 


x^  -  a (k) 


(6.54! 


ax  . 

-  x  (k) 


3x  A,.  ,  , 

— !  x ( k  j k-i: 


(6.55; 


(Xj^  -  a  (k)  )  2  +  x^ 


x^  -  a (k) 

( x ^  -  a  (k)  )  2  +  x^ 


(6.56) 


:  6 . 5  7  ; 


To  calculate  the  gains  of  the  filter,  the  following 


equation  is  used: 


G  (k)  =  P  (k  Ik-1)  H'  (k. 


{H  (k)  P  (k  I  k-1 )  H '  (k)  +  R (k)  J 


(6.58) 


After  some  manipulations  of  the  above  equation  in  its  matrix 
form,  it  is  found  that  the  gains  are  given  by  the  following 
equations : 

P.,  (k|k-l)H,  (k)+P.,(kjk-l)H_(k) 

G  (k)  =  - - - - - — - - - 5 - 

(k) PX1 (k | k-1) +hl (k) H2 (k) [P21 (k | k-1) +P12 (k | k-1) +h£ (k) P22 (k | k-1) +R  (k) ] 


•  .  v*  mj>  •  -  *  •  v'/SS  V  V.'  V.V.V.  V  * ^ »•- 


(6.59) 


P2i  (k ,  k-1)  Hx  (k)  +?22  (k ,  k-1)  H2  (k) 


G2 (k)  =  2 


.2 


H1 (k)  P11 (k  i k-1)  +H1 (k)  H2  (k)  fP21 (k  i k_1)  +P12  (k  i k_1)  +H2  (k)  P22  (k ;  k-1)  +R  (k)  ] 


(6.60) 


The  estimation  of  the  states  is  obtained  by  manipu¬ 
lating  the  following  equation: 


x(kjk)  =  x(k;k-l)  +  G(k)  (z(k)  -c(x(k  k-1)) 


(6.61) 


The  results  are: 


x^(k|k)  =  x^(k|k-l)  +  G^(k)z(k)  -  G^(k)tan 


_ 1  x2 (k  k-1 ) 

x1  (k  , k-1 )  -  a ( k ) 


(6.62) 


x2(k|k)  =  x2(k|k-l)  +  G2(k)z(k)  -  G2(k)tan 


-1 


x- (k | k-1 ) 


x1 (k | k-1)  -  a  (k) 


(6.63) 


The  error  covariance  matrix  during  the  filtering  phase 
of  the  Kalman  Filter  is  given  by  the  equation: 


P(kjk)  =  [I  G(k)H(k) ]P (k | k-1) 


(6.64) 


By  manipulating  this  equation  as  it  was  described  previously, 
the  following  equations  are  obtained: 
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P11(k,k)  =  PLl(k;k-l)  [1  -G1(k)Hi(k)]  -P21(k,k-l)G1(k)H2(k) 


(6.65) 


P12  (k  |  k)  =  [1  -G1(k)H1(k)]P12(k|k-l)  -G1(k)H2(k)P22(k  k-1) 


(6.66) 


P21  (k  ;  k)  =  [1  -G2  (k)H2  (k)  ]P21(k1k-l)  -G2(k)H1  (k)P11  (k  k-1) 


(6.67) 


P9 9  (k  j  k)  =  [1  -G2(k)H2(k)]P11(k;k-l)  -G^lklH,  (k)P0(k  k-1) 


(6.68) 


The  error  covariance  matrix  during  the  prediction 
phase  of  the  Kalman  Filter  is  given  by  the  equation: 


P (k | k-1)  =  A(k-l)P(k-l j  k-1) A'  (k-1)  +  Q (k-1 )  (6.69) 


This  yields  the  following  results: 


P, , (k | k-1)  =  P  (k-l|k-l) 


(6.70) 


P, ? (k | k-1)  =  P.,(k-l|k-l) 


(6.71) 


P,,(k|k-1)  =  P01 (k-1 Ik-1) 


(6.72) 


5»i 


P22 (k , k-1) 


P22(k-1  k-1) 


(6.73) 


The  normalized  state  error  squared: 

[x(k)  -x(k|k)]'P  ^(k)[x(k)  -x(k.k)] 

is  given  by  the  same  expression  as  in  the  previous  problem. 

All  the  above  results  are  used  in  a  computer  program 
(see  Appendix  A) .  This  program  basically  follows  the  steps 
of  an  Extended  Kalman  Filter  algorithm  as  it  was  described 
in  Chapter  III. 

From  the  outputs  it  can  be  seen  that  the  position 
variance  on  the  horizontal  axis  is  smaller  than  that  on  the 
vertical  axis.  This  was  expected  since  the  bearings  are 
crossed  with  relatively  small  angles  and  in  this  way,  there 
is  a  larger  uncertainty  on  the  vertical  axis. 

Also  from  the  results,  it  can  be  seen  that  the  esti¬ 
mates  for  x^ ,  x2,  are  improved  as  time  goes  on  and  finally, 
that  they  are  very  close  to  the  real  values. 

C.  MANEUVERING  TARGET  EXAMPLE 
1 .  Problem  Description 

The  target  is  modeled  by  the  following  equation: 


x(k+l) 


F  x  (k)  +  G  w  (k) 


(6.74) 


***********  St****  **  **** 

NGN-LINEAP  EXAMPLE  PUN 


This  equation  is  discretized  over  time  intervals  of  length 
T.  Using  Cartesian  coordinates,  the  state  is: 


x  =  (x  x  y  y]  ' 


(6.75) 


and 


1  T  0  0 

0  10  0 
0  0  1  T 

0  0  0  1  I 

_i 


(6.76) 


w  =  [w^  w2] 1 


(6.7-: 


T/2  0 

1  0 

0  T/2 

0  1 


(6.78) 


Eiw(k) ] 


0;  E[w(k)  w’ ( j) ] 


QS.  •  (6.79) 


The  initial  estimate  is  x ( 0  j  0 )  with  covariance  P(0j0). 
It  is  assumed  that  only  position  measurements  are 
available : 


z (k)  =  C  x (k)  +  v (k) 


(6.80) 


where 


(6. SI) 

(6.82) 

The  following  are  given: 

4  2 

T  =  10s,  Q  =  0,  =  R^2  =  10  m  ,  and 

2 

R^2  =  500m 

The  initial  conditions  of  the  target  are: 

x(0)  =  200m,  x(0)  =  0,  y(0)  =  10,000m,  y(0)  =  -15m  's 

The  target  is  on  a  constant  course  and  speed  until 

time  t  =  400s,  when  it  maneuvers  a  slow  90°  turn  in  the  x 

.  .  xv  2 

direction  with  acceleration  inputs  u  =  uJ  =  0.075m/s  .  It 

completes  the  turn  at  t  =  600s  (after  20  sampling  times)  and 

from  then  on  the  accelerations  are  zero.  The  second  turn, 

also  of  90°,  is  fast.  It  starts  at  t  =  610s  with  acceleration 
2 

of  0.3m/s  and  is  completed  at  t  =  660s,  i.e.,  after  5 
sampling  times. 

A  simulation  of  the  maneuvering  target  can  be  done 
in  x  coordinate  only  (the  results  for  the  y  coordinate  are 
similar) ,  using  the  method  of  "White  noise  model  with  adjusta¬ 


C  = 


0  0  10 


E (v(k) ]  =  0;  E[v(k)  v '  ( j )  ]  =  Ro 


ble  level. 


For  treating  this  problem,  a  linear  Kalman  Filter 


can  be  used.  The  only  difference  is  that  the  actual  trajec¬ 
tory  of  the  target  is  now  given  by  several  formulas  instead 
of  one.  The  different  phases  of  movement  of  the  target  A, 3, 
C,D,E,  can  be  formed  in  proper  mathematical  formulas,  which 
can  feed  the  position  measurements  one  after  the  other, 
depending  on  the  sampling  time. 

The  initial  estimates  of  the  states  in  the  x  direction 
were  obtained  as: 


x  (0  1 0)  =  x^ (0  ;  0)  =  (0) 

x  ( 0  |  0 )  =  x2 { 0 | 0 )  =  [z1  (0)  - z1  (-1) ]/T 

where 

z1(-l)  =  x(0)  -  x ( 0 ) T  +  vx (-1) 

(0)  =  x(0)  +  v^ (0) 

V  ~  N(0,R11) 

The  initial  covariance  matrix  is  then: 


P(0|0) 


Rn  Ru/T 

Rll/T  2rii/t2 

i  ns 


(6.83; 


(6.84) 


(6.85) 


(6.86) 


(6.87) 


(6.88) 


The  rest  of  the  calculations  needed  for  developing 
the  Kalman  Filter  algorithm  are  very  similar  to  those 
developed  in  the  first  example. 

If  the  maneuvers  are  not  taken  into  account  and  the 
filter  runs  as  usual,  the  tracking  results  are  very  poor 
during  the  maneuvers  (see  pp.  114-115). 

Using  the  "White  noise  model  with  adjustable  level" 
method,  a  self-ad j ustment  of  the  filter  can  be  accomplished 
as  follows: 

After  the  square  norm  of  innovations: 

£y(k)  =  V'(k)S_1V(k) 

exceeds  a  chosen  value,  the  target  is  considered  as  maneuver¬ 
ing  and  a  change  (increase)  in  Q,  is  made.  The  increase 
in  Q  increases  the  gain  G,  so  the  state  estimation: 

A  A  ^ 

x  (k | k)  =  x  (k | k-1 )  +  G(k)  (z(k)  -  c (k) x (k | k-1) ]  (6.89) 

is  more  dependent  on  the  second  term  since  the  state  estimation 
x(kjk-l)  is  less  reliable  during  the  maneuver.  The  filter 
continues  its  run  with  the  new  value  of  Q'  until  has  again 
a  lower  value  than  that  of  the  chosen  threshold.  Then  the 
filter  runs  again  with  the  given  initial  value  of  Q  (Q  =  0.0) . 

After  the  addition  of  the  above  provision  in  the 
tracking  computer  program,  the  results  were  much  better  than 
those  of  the  non-ad j ustable  filter. 


Three  categories  of  results  were  obtained.  In  each 
one,  different  values  of  Q's  and  threshold  levels  were  used. 


The  three  categories  are: 

a.  Observed  and  estimated  trajectories  versus  sampling 
time  (50  Monte  Carlo  runs) . 

b.  Observed  and  estimated  trajectories  versus  sampling 
time  (single  runs). 

c.  Mean  square  position  error  (50  Monte  Carlo  runs). 

The  above  results  (plots)  can  be  seen  on  pp.  116-149. 
A  tabulation  and  qualification  of  these  results  has  been 
developed  as  follows: 

Symbols 


S:  Constant  course  and  speed 

1M:  First  Maneuver 


2M 


Second  maneuver 


Very  Good 


The  estimated  trajectory  follows  the  time  trajectory 
with  great  reliability  (coincidence  of  the  paths) . 


Good:  The  estimated  trajectory  is  still  very  reliable, 

i.e.,  follows  the  true  path  closely  but  they 
don't  coincide  exactly. 


F.  Noise:  The  estimated  trajectory  follows  the  noise. 

Noise  E. :  The  estimated  trajectory  follows  the  noise 
exactly. 


Deviation ;  There  is  deviation  between  the  true  and  estimated 
trajectory. 


Deviation :  There  is  strong  deviation  between  the  true  and 

estimated  trajectory. 


TABLE  II 


Observed  and  Estimated  Trajectories 
(50  Monte  Carlo  Runs) 


0, 

.  1 

3 

20 

Very 

Good 

Very  Good 

Very  Good 

Deviation 

S.  Deviation 

S.  Deviation 

S,  Deviation 

S.  Deviation 

S.  Deviation 

Very 

Good 

Very  Good 

Good 

Good 

Deviation 

Deviation 

Deviation 

Deviation 

Deviation 

Very 

Good 

Very  Good 

Good 

Very 

Good 

Good 

Deviation 

Good 

Deviation 

Deviation 

Very 

Good 

Very  Good 

Good 

Very 

Good 

Good 

Deviation 

Good 

Good 

Deviation 

Very 

Good 

Very  Good 

Deviation 

Very 

Good 

Very  Good 

Deviation--F. 

Noise 

Very 

Good 

Good 

Deviation — F. 

Noise 
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TABLE  III 


Observed  and  Estimated 
(Single  Runs) 

0.1 


Tra jectoris 


Very  Good 
Deviation 
S.  Deviation 

F.  Noise 

Good 

Good 

F.  Noise  E. 

Good 

Good 

F.  Noise  E. 

F.  Noise  E. 

F.  Noise  E. 

F.  Noise  E. 

F.  Noise  E. 

F.  Noise  E. 


3 

Very  Good 
S.  Deviation 
S.  Deviation 

Very  Good 
Good 

Deviation 

F.  Noise 

Good 

Good 

F.  Noise 
Good 

F.  Noise 

F.  Noise 
Good 

F.  Noise 


20 

Very  Good 
S.  Deviation 
S.  Deviation 

Very  Good 
Deviation 
Deviation 

Good 

Deviation 

Deviation 

Good 

Deviation--F . 
Deviation--F . 

F.  Noise 
Deviation--F . 
Deviation--F . 


Noise 

Noise 


Noise  E. 
Noise  E. 
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Symbols 

1MAX: 

2MAX: 

3MIN: 


Maximum  value  of  m.s.e.  during  the  first 
maneuver 

Maximum  value  of  m.s.e.  during  the  seconc 
maneuver 

Minimium  value  of  m.s.e.  during  movement 
under  constant  course  and  speed. 


MAN:  Maneuvers 


»vW.«<A 
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TABLE  V 


Conclusions  From  Tables  II,  III,  IV 


TABLE  II 


TABLE  III 


High  Q 


S  Good 


MAN  Very  Good 


F.  Noise 
F.  Noise  E. 
F.  Noise 
F.  Noise  E. 


Low  Q 


S  Very  Good 


MAN  S.  Deviation 


Very  Good 

Deviation 
S.  Deviation 


S  Deviation 

High 

Threshold 

MAN  Deviation 


Very  Good 
Good 

Deviation 
S.  Deviation 


S  Very  Good 

Low 

Threshold 

MAN  Very  Good 


F.  Noise 
F.  Noise  E. 
Good 

F.  Noise 


TABLE  IV 

r.m.s.e. 

increases 

r.m.s.e. 

decreases 

r.m.s.e. 

decreases 

r.m.s.e. 

increases 

r.m.s.e. 

decreases 

r.m.s.e. 

increases 

r.m.s.e. 

increases 

r.m.s.e. 

decreases 


0B5.  AND  E5T1M.TRAJ.  NON- ADJUST.  I 
CONT1N.  LINE:  MEBSUREBENTS  TRBJECT 
0BS*£0  LINE:  r ILTER  S  ESTIN.  TRBjCC 


OBS.  AND  ESTlh.TRAj.  NON -ADJUST.  FILTER 
SINGLE  RUN  OF  THE  PROGRAM 
CONTIN.  LINE;  MEASUREMENTS  TRRjECT. 

DASHED  LINE:  FILTER  S  ESTJM.  TRAjECT. 


mg 


OBSERVED  FIND  ESI ! M . TRflJECT .  0-1  CO  TR- 
C0NT1N.  LINE:  MEASUREMENTS  TRflJECT. 
DASHED  L1NC:  FILTER  S  EST1M.  TRflJECT. 


Observed  and  list  i in.  Trajt 
50  Monte  Carlo  Runs 


OBSERVED  AND  ESI I M . TRfUECT .  O-lOO  TR-20 
C0NT1N.  LINE:  MEASUREMENTS  TRfUECT. 

DASHED  LINE:  FIlTER  S  ESTIH.  TRAJECT. 


Observed 


OBSERVED  RND  EST1M.  TRRJf.CT .  C-.l  ‘ 
SINGLE  RUN  or  THE  PROGRAM 
CONTiN.  LINE:  MEASUREMENTS  TRfUECT. 
DASHED  LINES  FILTER  S  ESTiM.  TRAjECT. 


Observed 


OBSERVED  AND  EST3H.  TRAJLCT.  0-3  TR 

SlNGLC  RUN  or  THC  PROGRAM 
CONTJN.  LINE:  MEASUREMENTS  TRAjECT. 
DASHED  LINE:  f 1LTER  S  ESTIH.  TRAjECT. 


Observed  and  listiin.  Ti a  joe 
Single  Run  of  the  Program 


SINGLE  RJN  OT  ThC  PROGRPfl 
N.  i_!NE:  NEeS'jREMCNTS  TRRjECT. 

I  LINEi  flLTER  S  lSTIH.  TRRjECT 


OBSERVED  RND  ESTIM.  TRPJECT .  O-lOO 
SINGLE  RUN  Or  T*C  PROGRAM 
COST  IN.  LINE:  MEASUREMENTS  TRPjECT. 
OASHEC  LINE:  FILTER  S  ESTIM.  TRAJECT 


I’roijrani 


OBSERVED  PND  ESTIM.  TRPJECT.  C-lOOO  TR 
SINGLE  RUN  or  THC  PROGRAM 
CONTIN.  LINE :  MEASUREMENTS  TRP.jCCT. 
DASHED  LINE:  f ILTER  S  EST!M.  TRfl JECT . 


Observed  and  blstim.  Trajm 
Single  Run  of  the  Program 


illM.  TRBJECT.  0-. 
RUN  or  THE  PROGRAM 
:  MEASUREMENTS  TRAjECT 
riLTER  S  EST1M.  TRAJEC 


Figure  6.37  Observed  and  list  im.  Tru  jot: 

Single  Run  of  the  Program 


OBSERVED  AND  ESTltt.  TRAJECT.  O-IO  TR-3 
SINGLE  sun  gt  the  PR0GRR1 
COST  IN.  LINE:  HERSIJREHEHT5  TRB-ECT. 

OflSHEO  LINEi  flLTER  S  ESTIfl.  TRftJCCT. 


figure  6.39  Observed  and  bst.  im.  Ti  a  jec 
Single  Run  of  Lhe  I'rog ruin 


OBSERVED  RMO  EST1M.  TRBJECT .  G-1GOO  TR-3 

SINGLE  RUN  Of  THE  PROGRAM 
CONTiN.  LINE:  MEASUREMENTS  TRftJECT. 

OflSHCD  LINE:  flLTER  S  ESTIM.  TRRjECT. 


1  m 


OBSERVED  RNQ  EST1M.  TRflJECT .  0-  1C  TR-20 

SINGLE  RUN  OT  THE  PROGRAM 
CONI IN.  LINE:  MEASUREMENTS  TRfuECT. 

ORSHED  LINE:  TILTCR  S  ESTIM.  TRRjECT. 


IK.  ■RfiJlTT.  0-1 OCC 

RUN  Of  THE  PR0GRRH 
•  RCftSUROlENTS  TRfUECT. 
FILTER  S  ESTiM.  TROJCCT 


Observed  and  Estim.  Traced. 
Single  Run  of  the  Program 
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l'iyure  6.47  Mean  Square  Error 


Mean 
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lea 
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Fiqure  6.51  Mean  Square  Error  of  Pos .  0 '  =  1000  TK 


yure 


MERN  SOURRE  ERROR  OF  POSIT. 


I’iyure  6.54  Mean  Square  Error  of  I’ns. 
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MEAN  SQUARE  ERROR  QE  POS.  QM  TR-20 


lea 


;e:HN  50URRE  ERROR 


Kiyure  6.62  Mean  Square  Error  of  I’os.  O'  1000  Tl< 


Since  the  observed  and  estimated  paths  in  the  Monte 


Carlo  case  are  averaged,  the  tracking  results  are  better 
(smoother)  and  it  is  not  easy  to  observe  the  various  affec¬ 
tions  of  extreme  values  of  Q  and  the  threshold  level.  For 
this  reason,  only  three  characteristic  plots  of  this  case 
are  presented  and  the  following  conclusions  are  based  mainly 
on  the  other  two  cases: 

a.  When  the  target  is  moving  with  constant  course  and 
speed,  it  is  desirable  to  have  low  values  of  Q  and 
high  threshold  level. 

b.  When  the  target  is  maneuvering,  it  is  desirable  tc 
have  medium  or  high  values  of  Q  and  low  threshold 
level . 

Based  on  the  above  results,  an  attempt  was  made  to 
run  the  filter  using  three  levels  of  process  noise  Q  (two 
threshold  levels) .  In  this  way  the  two  types  of  maneuvers 
(slow-fast)  were  treated  separately.  The  threshold  levels 
chosen  were  1  and  5.  The  mean  square  error  of  position  and 
velocity  in  the  x  direction  over  50  Monte  Carlo  runs  was 
plotted.  The  results  (pp.  152-171)  were  very  good  especially 
for  the  position  m.s.e.  A  tabulation  of  these  results  follows 
as  Table  VI. 
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TABLE  VI 


b 


Multiple  Process  Noise  Levels  Results 


R.M.S.  Position  Error 

R.M.S.  Velocity  Error 

THR. 

Q' 

1-5 

1-5 

;!>•> 

1MAX 

112 

13.2 

-~ni 

0,0.1,100 

2MAX 

140 

14 

3MIN 

35 

0.4 

1MAX 

94 

9.4 

0,3,100 

2  MAX 

124 

13.6 

3MIN 

45 

1.0 

* 

1MAX 

88 

9.6 

0,10,100 

2  MAX 

102 

13.9 

3MIN 

50 

1.4 

1MAX 

160 

11.4 

0,0.1,10 

2  MAX 

208 

13.6 

r 

3MIN 

35 

0.4 

1MAX 

115 

10 

o 

H 

r-H 

o 

2  MAX 

168 

13.2 

3MIN 

40 

0.6 

lMAX 

96 

10 

•/ 

0,3,10 

2  MAX 

140 

13.4 

.‘v'\ 

3MIN 

44 

0.8 

lMAX 

136 

21.5 

0,0.1,1000 

2  MAX 

140 

20.2 

3MIN 

37 

0.25 

lMAX 

112 

16 

\  *‘.v 

0,1,1000 

2MAX 

130 

17.4 

**v 

3MIN 

44 

0.7 

lMAX 

100 

13.5 

0,3,1000 

2  MAX 

120 

15.3 

3MIN 

46 

1.0 

lMAX 

89 

10.6 

0,10, 1000 

2MAX 

106 

15 

3MIN 

50 

1.3 
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MEAN  SQUARE  POSITION  ERROR 
MANY  Q  LEVELS 
Q  LCVClSi  O-I-IO 


SAMPLING  TIME 


MEAN  SQUARE  POSITION  ERROR 
NfiNT  0  LEVELS 

Q  LEVELS i  0-3-10 


1'iqure  6.68  Mean  Square  Position  Krror;  Many  (.>’ 
Q’  Levels :  0,3,10 
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MANY  0  LEVELS 


SAMPLING  TIME 


MEAN  SQUARE  POSITION  ERROR 
MANY  Q  LEVELS 

0  LEVELS i  O-l-lOOO 


SAMPLING  TIME 


EfiN  SOUflRE  POSITION  ERROR 
MflNr  Q  LEVELS 
0  LEVELSi  0-3-1000 


ING  TIME 
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THRES.  LEVELS:  1-5 
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Figure  6.79  Mean  Square  Velocity  Krror;  Many 
O'  Levels:  0,1,1000 
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MANY  0  LEVELS 
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THRES.  LEVELS:  l'S 
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Bar-Shalom  [Ref.  9],  has  developed  a  different  approach 
for  the  same  problem.  In  this  case  the  tracking  filter 
operates  in  its  normal  mode  in  the  absence  of  any  maneuver. 

Once  a  maneuver  is  detected,  a  different  state  model  is  used 
by  the  filter.  The  acceleration  is  added  as  a  new  state 
component.  The  extent  of  the  maneuver  as  detected,  is  then 
used  to  yield  an  estimate  for  the  extra  state  component, 
and  corrections  are  made  on  the  other  state  components.  The 
tracking  is  then  done  with  the  augmented  state  model  until 
it  will  be  converted  to  the  normal  model  by  another  decision. 

The  results  of  this  approach  can  be  seen  in  Figure 
6.83.  The  curve  having  the  indication  "VD"  belongs  to  the 
above  algorithm  while  the  other  curve  (indication  "IE") 
belongs  to  a  different  approach  developed  by  Chan  [Ref.  10]. 

The  outline  of  the  latter  algorithm  is  the  following: 

When  a  maneuver  is  detected,  the  magnitude  of  the 
acceleration  is  identified  in  a  least  squares  format.  The 
result  is  used  in  conjunction  with  a  standard  Kalman  Filter 
to  estimate  the  state  of  the  vehicle.  The  aim  of  the  accelera¬ 
tion  input  estimation  is  to  remove  the  filter  bias  caused 
by  the  target  deviating  from  the  assumed  constant  velocity, 
straight  line  motion. 

It  can  be  easily  see  (Figure  6.83)  that  the  algorithm 
of  Reference  9  is  superior  to  that  of  Reference  10. 

The  "white  noise  model  with  adjustable  level"  method, 
modified  with  multiple  levels  of  process  noise  Q  (one  for 


each  type  of  maneuver) ,  can  be  compared  with  the  algorithms 
of  References  9  and  10,.  Specifically  this  paper's  algorithm 
is  better  than  both  the  algorithms  in  the  r.m.s.  position 
error  results  and  worse  in  the  r.m.s.  velocity  error  results 
In  the  r.m.s.  velocity  error  case,  the  approximate  maximum 
value  of  the  two  other  algorithms  is  11,  while  this  paper's 
algorithm  maximum  value  is  13.3  (not  too  big  a  difference). 
In  the  r.m.s.  position  error  case,  the  maximum  values  for 
References  9  and  10  algorithms  are  200  and  125,  respectively 
while  this  paper's  algorithm  has  achieved  an  approximate 


value  of  95. 


VII.  CONCLUSIONS 


In  the  examples  presented,  it  was  seen  that  the  Kalman 
Filter  has  the  ability  and  flexibility  to  treat  various 
cases  of  the  target  tracking  problem  with  very  satisfactory 
results . 

The  Kalman  Filter  is  always  initialized  by  the  user, 
providing  the  initial  estimate  x(k)  and  its  corresponding 
estimate  error  covariance  matrix  P£k).  The  initialization 
is  of  great  importance  for  the  filter's  performance.  A  poor 
initialization  needs  more  observations  and  time  for  the 
algorithm  estimate  to  converge  toward  the  value  of  the  state 
vector . 

In  all  cases  the  importance  of  the  gain  matrix  G{k)  of 
the  Kalman  Filter  was  significant,  i.e.,  since  it  is  inversely 
proportional  to  time,  it  weights  the  correction  term 

A 

[z(k)  -x(k|k-l)]  less  heavily  as  time  progresses,  and  so  the 

A 

state  estimation  x(k|k)  depends  more  on  the  state  estimation 
of  past  time  x(k|k-l). 

It  was  also  noted  that  the  Q  matrix  accounts  for  any 
model  inaccuracies.  For  a  filter  in  a  steady  state  condition, 
the  Q  also  serves  to  prevent  the  gain  matrix  Q.(k)  from 
approaching  zero  by  always  insuring  uncertainty  in  the  pre¬ 
dicted  error  covariance  matrix.  Q  is  also  the  key  variable 
for  treating  the  maneuvering  target  problem  with  the  "White 


noise  model  with  adjustable  level"  method.  As  it  was  ex¬ 
plained  in  the  corresponding  chapter,  variations  in  Q  cause 
proportional  variations  to  the  gain  G(k) ,  which  weights  the 
correction  factor  in  the  equation: 

x(kjk)  =  x(kjk-l)  +  G(k)[z(k)  -  c (k)  x (k . k-1)  ] 

Larger  values  of  Q  tend  to  cause  the  state  estimate  to  favor 
the  observation. 

The  error  covariance  matrix  and  specifically  some  of 
its  elements  (position  and  velocity  error  variance,'  ,  were 
very  useful  as  indicators  of  the  filter's  performance. 

In  the  case  of  the  maneuvering  target,  the  "White  noise 
model  with  adjustable  level"  method  was  very  reliable  and 
comparable  to  the  algorithms  of  References  9  and  10.  Addi¬ 
tionally,  it  is  much  simpler  than  the  other  two  methods.  The 
only  disadvantage  was  the  greater  value  of  the  velocity  r.m.s. 
error.  In  the  example  presented,  the  difference  was  not 
significant  but  it  is  estimated  that  it  would  increase  in 
cases  where  the  target  maneuvers  more  drastically  (greater 
accelerations) . 

The  Monte  Carlo  simulation  should  be  used  to  provide 
statistical  results  for  any  stocahstic  process  which  is 
represented  by  pseudorandom  numbers.  This  simulation  is  a 
rigorous  statistical  procedure,  to  compare  two  or  more 
algorithms . 


More  specific  conclusions  concerning  the  examples  presented 
have  already  been  provided  in  the  corresponding  analyses  of 
them. 

It  is  suggested  that  a  continuation  of  this  research 
should  include  the  following: 

a.  Problem  of  maneuvering  target  which  uses  greater 
acceleration  driving  the  maneuvers.  The  "White  noise 
model  with  adjustable  level'1  method  should  be  applied  and 
the  results  in  position  and  velocity  r.m.s.  errors  should  be 
obtained . 

b.  Problem  of  multiple  targets  under  various  situations 
(maneuvering,  in  clutter,  etc.). 

c.  Problem  of  single  and  multiple  maneuvering  targets 
in  clutter.  It  is  an  interesting  and  important  topic  not 
included  in  this  paper  due  to  lack  of  time. 

d.  Study  the  implementatiqn  of  a  Kalman  Filter  algorithm 
using  special  purpose  hardware. 
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